#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/Pose.h>
#include <moveit_msgs/RobotTrajectory.h>
#include <tf2/LinearMath/Quaternion.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "draw_circle");
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(1); // 使用异步 spinner 处理回调
    spinner.start();

    // 初始化 MoveIt 接口
    static const std::string PLANNING_GROUP = "manipulator"; // 修改为你的规划组名称
    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
    move_group.setMaxAccelerationScalingFactor(0.8);
    move_group.setMaxVelocityScalingFactor(0.8);

    // 输出一些信息
    ROS_INFO("Reference frame: %s", move_group.getPlanningFrame().c_str());
    ROS_INFO("End effector link: %s", move_group.getEndEffectorLink().c_str());

    // 1. 机械臂移动到固定的起始位置
    geometry_msgs::Pose start_pose;
    start_pose.position.x = 0.3;  // 固定点 X 坐标
    start_pose.position.y = 0.0;  // 固定点 Y 坐标
    start_pose.position.z = 0.1;  // 固定点 Z 坐标
    start_pose.orientation.w = 1.0; // 默认方向
    move_group.setPoseTarget(start_pose);

    ROS_INFO("Moving to the start position...");
    if (move_group.move() != moveit::planning_interface::MoveItErrorCode::SUCCESS)
    {
        ROS_ERROR("Failed to move to the start position.");
        return 1;
    }

    // 2. 绘制圆的笛卡尔路径
    ROS_INFO("Planning circular path...");
    std::vector<geometry_msgs::Pose> waypoints;
    const double center_x = 0.3 ;  // 圆心 X 坐标
    const double center_y = 0.0;   // 圆心 Y 坐标
    const double center_z = 0.1;   // 圆心 Z 坐标
    const double radius = 0.05;     // 圆的半径
    const int num_points = 1000;    // 圆的分辨率，越大越平滑

    // 生成圆的路径点(xy 平面)
    for (int i = 0; i <= num_points; ++i)
    {
        double angle = 2 * M_PI * i / num_points; // 当前角度
        geometry_msgs::Pose waypoint = start_pose; // 以起始位置为基准
        waypoint.position.x = center_x + radius * cos(angle); // 圆周 X 坐标
        waypoint.position.y = center_y + radius * sin(angle); // 圆周 Y 坐标
        waypoint.position.z = center_z; // 圆周 Z 坐标保持不变
        waypoints.push_back(waypoint);
    }

    // 使用 MoveIt 规划笛卡尔路径
    moveit_msgs::RobotTrajectory trajectory;
    double fraction = move_group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory);
    if (fraction < 1.0)
    {
        ROS_WARN("Circular path planning incomplete (%.2f%% achieved).", fraction * 100.0);
    }
    else
    {
        ROS_INFO("Circular path planning successful (100%% achieved).");
    }

    // 3. 执行路径
    ROS_INFO("Executing circular path...");
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    plan.trajectory_ = trajectory;
    if (move_group.execute(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS)
    {
        ROS_ERROR("Failed to execute circular path.");
        return 1;
    }

    ROS_INFO("Circular motion completed!");

    ros::shutdown();
    return 0;
}
